/*PID控制器*/
#include <iostream>
#include "simucpp.hpp"
using namespace simucpp;
using namespace std;

class PID_Controller: public PackModule
{
public:
    PID_Controller() {};
    PID_Controller(Simulator *sim, double Kp=1, double Ki=0, double Kd=0, double N=100)
        :_Kp(Kp), _Ki(Ki), _Kd(Kd), _N(N) {
        Initialize(sim);
    };
    virtual PUnitModule Get_InputPort(uint32_t n) const override {
        if (n==0) return mdin1;
        return nullptr;
    };
    virtual PUnitModule Get_OutputPort(uint32_t n) const override {
        if (n==0) return mdout1;
        return nullptr;
    };
    void Initialize(Simulator *sim){
        mdintegral = new UIntegrator(sim, "mdintegral");
        mddifferential = new TransferFcn(sim, vecdble{1, 0}, vecdble{1, _N}, "mddifferential");
        mdin1 = new UGain(sim, "mdin1");
        mdout1 = new USum(sim, "mdout1");
        sim->connectU(mdin1, mdintegral);
        sim->connectU(mdin1, mddifferential, 0);
        sim->connectU(mdin1, mdout1);
        sim->connectU(mdintegral, mdout1);
        sim->connectU(mddifferential, 0, mdout1);
        mdout1->Set_InputGain(_Kp, 0);
        mdout1->Set_InputGain(_Ki, 1);
        mdout1->Set_InputGain(_Kd, 2);
    }
    double _Kp=1, _Ki=0, _Kd=0, _N=100;
private:
    UIntegrator *mdintegral;
    TransferFcn *mddifferential;
    UGain *mdin1;
    USum *mdout1;
};

int main()
{
    Simulator sim1;
    auto *mdclose = new TransferFcn(&sim1, vector<double>{1}, vector<double>{1, 1, 1});  // 闭环传函
    auto *mdpid = new PID_Controller(&sim1, 1, 0, 9, 1);  // PID控制器
    auto* mdGs = new TransferFcn(&sim1, vector<double>{1}, vector<double>{10, 1, 0});  // 被控对象
    FUInput(mdu, &sim1);
    FUSum(mdsum, &sim1);
    FUOutput(mdout1, &sim1);
    FUOutput(mdout2, &sim1);
    mdu->Set_Function([](double u){return 1;});

    sim1.Set_SimStep(1/double(1<<10));
    sim1.connectU(mdu, mdclose, 0);
    sim1.connectU(mdu, mdsum);
    sim1.connectU(mdsum, mdpid, 0);
    sim1.connectU(mdpid, 0, mdGs, 0);
    sim1.connectU(mdGs, 0, mdsum);
    sim1.connectU(mdGs, 0, mdout1);
    sim1.connectU(mdclose, 0, mdout2);
    mdsum->Set_InputGain(-1);

    sim1.Initialize();
    sim1.Simulate();
    sim1.Plot();
    cout.precision(12);
    cout << mdout1->Get_OutValue() << endl;
    cout << mdout2->Get_OutValue() << endl;
    return 0;
}
